#ifndef ARM_CONTROL__ARM_ACTION_TYPE10_HPP_
#define ARM_CONTROL__ARM_ACTION_TYPE10_HPP_

#include "arm_control/arm_action_base.hpp"
#include "fruit_detector/msg/detection_array.hpp"
#include "std_msgs/msg/int32.hpp"
#include <moveit/move_group_interface/move_group_interface.h>
#include "rclcpp_action/rclcpp_action.hpp"
#include "voice_planner/action/voice_planner.hpp"
#include <vector>
#include <limits>
#include <cmath>

namespace arm_control
{

// 水果检测数据结构
struct FruitData10 {
  int32_t type;
  float offset_x;
  float offset_y;
  bool isgood;
  
  // 计算offset (使用欧几里得距离)
  float get_offset() const {
    return std::sqrt(offset_x * offset_x + offset_y * offset_y);
  }
};

// 动作类型10：右臂抓取并放置动作
class ArmActionType10 : public ArmActionBase
{
public:
  explicit ArmActionType10(rclcpp::Logger logger, rclcpp::Node::SharedPtr node);
  
  void execute(
    const std::shared_ptr<GoalHandleARM> goal_handle,
    std::shared_ptr<ARM::Feedback> feedback,
    std::shared_ptr<ARM::Result> result) override;

private:
  rclcpp::Node::SharedPtr node_;
  rclcpp::Subscription<fruit_detector::msg::DetectionArray>::SharedPtr right_sub_;
  
  // 爪子发布器
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr zhuazi_right_publisher_;
  
  // MoveIt 接口
  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_right_;
  
  // VoicePlanner action 客户端
  using VoicePlanner = voice_planner::action::VoicePlanner;
  rclcpp_action::Client<VoicePlanner>::SharedPtr voice_planner_client_;
  
  // 存储订阅的数据
  std::vector<FruitData10> right_data_;
  
  // 回调函数
  void right_callback(const fruit_detector::msg::DetectionArray::SharedPtr msg);
  
  // 辅助函数：根据类型数组选择offset最小的水果
  FruitData10 select_best_fruit(
    const std::vector<int32_t>& type_array,
    const std::vector<FruitData10>& data,
    bool& found);
  
  // 根据offset调整关节值并进行运动规划
  bool move_with_joint_adjustment(
    double offset_x,
    double offset_y);
  
  // x轴正向移动指定距离
  bool move_x_positive(double distance);
};

} // namespace arm_control

#endif // ARM_CONTROL__ARM_ACTION_TYPE10_HPP_

